#ifndef __BOOLEAN_MESH_
#define __BOOLEAN_MESH_


#include <vector>
#include <map>
#include <deque>
#include <boost/lambda/lambda.hpp>

#include "common.h"
#include "mesh.h"
#include "aabbtree.h"

#include "bm_primitive.h"


// This structure is used for merging intersection. 
extern std::map <std::pair <unsigned, unsigned>, IntersectionRecord<Value> > map_mergeInter;

template <typename BooleanMeshPrimitive>
class BooleanMesh : public Mesh <BooleanMeshPrimitive>
{
public:
	class SegmentStruct ;
public:
	typedef typename BooleanMeshPrimitive::ValueType              ValueType; 
	typedef typename BooleanMeshPrimitive::FaceType               FaceType;
	typedef typename BooleanMeshPrimitive::VertexType             VertexType;
	typedef typename BooleanMeshPrimitive::HalfEdgeType           HalfEdgeType ; 
	typedef typename BooleanMeshPrimitive::EdgeType               EdgeType ; 
	typedef typename BooleanMeshPrimitive::VectorType             VectorType ;
	typedef typename BooleanMeshPrimitive::PointType              PointType ;
	typedef typename BooleanMeshPrimitive::MeshType               MeshType ;
	typedef typename Mesh<BooleanMeshPrimitive>::HalfEdgeIterator HalfEdgeIterator ;
	typedef typename Mesh<BooleanMeshPrimitive>::VertexIterator   VertexIterator ;
	typedef Segment<ValueType>                                    SegmentType ;
    typedef Ray<ValueType>                                        RayType ;
		

	typedef AABBTreePoly <ValueType>                         AABBTreePolyType; 
	typedef AABBTreePolyBuilder <ValueType>                  AABBTreePolyBuilderType  ; 
	typedef AABBTreePolygon <ValueType>                      AABBTreePolygonType ;
	typedef AABBTreeNode <ValueType> AABBTreeNodeType ;
	typedef Box<ValueType> BoxType; 
	typedef Polygon<ValueType> PolygonType; 

    typedef typename AABBTreePolyType::PolygonColResult           PolygonColResult;
    typedef typename AABBTreePolyType::RayColResult               RayColResult;

	typedef typename std::multimap<unsigned, unsigned>::iterator  MapUUIterator ;

public:
	enum PARTTYPE {IN, OUT}; 
		
public :
	/**
	 * Default constructor. 
	 */
	BooleanMesh (): tree (NULL) {}

	/**
	 * Virtual destructor. 
	 */
	virtual ~ BooleanMesh() 
	{
		if (tree !=NULL)
			delete tree ;
	}

	/**
	 * Mesh constructor. 
	 */
	void construct()
	{
		for (HalfEdgeIterator he_iter  = this->hash_he.begin(); he_iter != this->hash_he.end(); ++he_iter)
		{
			he_iter->second->mp_mesh  = (MeshType *) (this) ;
		}

		for (VertexIterator v_iter  = this->vertices.begin(); v_iter != this->vertices.end(); ++v_iter)
		{
			v_iter->second->mp_mesh  = (MeshType *) (this) ;
		}
	}

	/**
	 * Build aabbtree from the polygon in mesh. 
	 * @post The coordinates of vertices cannot be changed. Otherwise, the tree must be rebuilt. 
	 */
	AABBTreePolyType * build_aabbtree() 
	{
		if (tree != NULL) delete tree; 
		AABBTreePolyBuilderType builder  ; 
		this->tree = builder.build_from_mesh ((Mesh<MeshPrimitive<ValueType> > &)(*this) ,1);
		return this->tree; 
	}

	/**
	 * Get the aabbtree pointer. 
	 * @return the aabbtree pointer. 
	 */
	AABBTreePolyType * get_aabbtree () const 
	{
		return this->tree; 
	}

	/**
	 * test whether a point is in or out of the B-rep. 
	 * Now we dont reply to on the boundary. 
	 * @param _p the point to query
	 * @param _normal the direction of the testing ray. 
	 - Actually, we should sort the polygon intersected. Here we only pick on face and compare with its normal for simplicity. 
	*/
	bool is_in_volumn (const PointType & _p, const VectorType & _normal )
	{
		RayType ray (_p, _normal);
		RayColResult rayColRes;
		tree->collideWithRay (ray, rayColRes);
		/*
		Value d = Value(1);
		for (unsigned i = 0; i < rayColRes.polys_.size(); ++i)
		{
			const AABBTreePolygon<Value> * p = rayColRes.polys_[i];
			d *= (p->get_face()->normal() * _normal);
			d *= Value(-1);
		}
		return d < 0;
		*/
		std::cout<<"Query Point Normal"<<_p<<_normal<<std::endl; 
		std::cout<<"Size "<<rayColRes.polys_.size()<<std::endl;	   
		IntersectResult<ValueType> res; 
		if (rayColRes.polys_.size() > 0)
		{
			unsigned idx = -1; 
			ValueType distance = large_value; 
			for (unsigned i = 0; i < rayColRes.polys_.size(); ++i)
			{   std::cout<<"Normal"<<rayColRes.polys_[i]->get_face()->normal()<<std::endl;
				const PolygonType & polygon = ((FaceType *) rayColRes.polys_[i]->get_face())->get_polygon(); std::cout<<polygon<<' '<< ((FaceType *) rayColRes.polys_[i]->get_face())->m_idx<<std::endl;
				intersect_ray_triangle (ray, polygon, res);
				if (res.res_type == POINT)
				{ std::cout<<"Intersect point "<<res.point<<std::endl;
					ValueType d = (res.point - _p).square_module();
					if ( d < distance)
					{
						idx = i; 
						distance = d ;
					}
				}
				else
				{
					throw ("Fail in find the nearest intersection point");
				}
			}
			const AABBTreePolygon<Value> * p = rayColRes.polys_[idx];
			VectorType faceNormal = p->get_face()->normal() ;
			return (faceNormal * _normal >= 0 );
		}
		return false ;
	}



    /**
     *
     */
	FaceType * construct_face2 (MeshType & mesh, const std::vector <unsigned > & v_id) 
	{
		unsigned v_size = v_id.size(); 
	 	FaceType * p_face = new FaceType; 
		std::vector<HalfEdgeType * > v_he; 
		for (unsigned i = 0; i < v_size; ++ i)
		{
			v_he.push_back(new HalfEdgeType);
		}

		for (unsigned i = 0; i < v_size; ++i)
		{
			v_he[i]->face = p_face ;
			v_he[i]->prev = v_he[(i + v_size - 1) % v_size] ;
			v_he[i]->next = v_he[(i + 1) % v_size] ;
			
			v_he[i]->from_v = mesh.vertices[v_id [i]];
			mesh.vertices[v_id[i]]->he = v_he[i] ;

			mesh.hash_he [HashKey (v_id[i], v_id[(i + 1 ) % v_size])] = v_he[i] ;
		}

		p_face->he = v_he[0] ;
		p_face->m_idx =  this->faces.size();
		mesh.faces.push_back(p_face);


		// Update the flips and Edges 
		for (unsigned i = 0; i < v_size; ++i)
		{
			unsigned id1 = v_id[i];
			unsigned id2 = v_id[ (i + 1) % v_size] ;

			HalfEdgeIterator iter ;

			iter = mesh.hash_he.find(HashKey(id2, id1)) ;
			if (mesh.hash_he.end() == iter)
			{
				v_he[i]->flip = NULL;
				EdgeType * edge = new EdgeType;
				edge->he = v_he[i] ;
				v_he[i]->edge = edge;
				if (id1 < id2) mesh.edges[HashKey(id1, id2)] = edge;
				else mesh.edges[HashKey(id2, id1)] = edge;
			}
			else
			{
				v_he[i]->flip = (*iter).second;
				(*iter).second->flip = v_he[i] ;
				v_he[i]->edge = (*iter).second->edge ;
			}
			
		}

		p_face->construct(); 
		return p_face; 
	}

	/**
	 * create the new face for new mesh. 
	 * @param new_mesh The reference of new mesh. 
	 * @param v_he Vector of the old face in counterclock wise order. 
	 * @param vertex_map The map from the Old vertex point to  the  NEW vertex pointer in pairwise. 
	 */
	void copy_face (MeshType & new_mesh, const std::vector<HalfEdgeType *> & v_he, std::map <VertexType *, VertexType * > & vertex_map, std::map <VertexType *, VertexType * > & boundary_vertex_map, VectorType & normal) 
	{
		static unsigned vertex_id = 0; 
		std::vector<unsigned> v_vertex_id ;
		// Step 1. Create New Vertex. If the Vertex Exist
		for (unsigned i = 0; i < v_he.size(); ++i)
		{


			VertexType * old_vertex = (VertexType *)v_he[i]->from_v ;
			// Substitute the boundary vertex. 
			typename std::map <VertexType *, VertexType * >::iterator boundary_iter_pos = boundary_vertex_map.find (old_vertex);
			if ( boundary_iter_pos != boundary_vertex_map.end())
			{
				//std::cout<<"FIND"<<std::endl;
				old_vertex = boundary_iter_pos->second ;
			}

			typename std::map <VertexType *, VertexType * >::iterator iter_pos = vertex_map.find (old_vertex) ;
			if ( iter_pos == vertex_map.end())
			{
				VertexType * pv = new VertexType ;
				pv->m_coord = old_vertex->m_coord ;
				pv->m_idx = vertex_id++ ;
				new_mesh.vertices[pv->m_idx] = pv ;
				v_vertex_id.push_back(pv->m_idx);

				vertex_map.insert(std::make_pair(old_vertex, pv));
			}
			else
			{
				v_vertex_id.push_back(iter_pos->second->m_idx);
			}
		}
		for (unsigned i = 0 ; i < v_vertex_id.size(); ++i)
		{
			std::cout<<v_vertex_id[i]<<' ';
		}
		std::cout<<std::endl;
		// Step 2. Construct the face 
		FaceType * p_face = this->construct_face2(new_mesh, v_vertex_id); 
		// Step 3. Set face normal 
		p_face->normalVector = normal;
	}

	/**
	 * Copy mesh by traversing halfedges only. 
	 * @new_mesh The reference of the new empty mesh. 
	 */
	void copy_mesh (MeshType & mesh1, MeshType & mesh2, FaceType * begin_face , MeshType & new_mesh, 	std::map<unsigned, unsigned > & vv_relation) 
	{
		// build boundary vertex_map; 
		std::map <VertexType *, VertexType * > boundary_vertex_map ;
		typename std::map <std::pair <unsigned, unsigned>, IntersectionRecord<Value> >::iterator iter1 ;
		typename std::map <std::pair <unsigned, unsigned>, IntersectionRecord<Value> >::iterator iter2 ;

		typename std::map<unsigned, unsigned>::iterator vv_iter ;
		for (vv_iter = vv_relation.begin(); vv_iter !=vv_relation.end(); ++vv_iter )
		{
			boundary_vertex_map.insert (std::make_pair (mesh1.vertices[vv_iter->first], mesh2.vertices[vv_iter->second]));
		}
/*
		for (iter1 = mesh1.map_mergeInter.begin(); iter1 != mesh1.map_mergeInter.end(); ++iter1)
		{
			std::pair<unsigned, unsigned> key2 = iter1->first ;
			swap (key2.first, key2.second);
			HalfEdgeType * he1 = iter1->second.v_merge_he[0].first ;
			iter2 = mesh2.map_mergeInter.find (key2);
			if (iter2 == mesh2.map_mergeInter.end()) throw ("Find corresponding key failed.");
			HalfEdgeType * he2 = iter2->second.v_merge_he[0].first ;

			boundary_vertex_map.insert (std::make_pair((VertexType *)he1->from_v , (VertexType *)he2->next->from_v ));
			boundary_vertex_map.insert (std::make_pair((VertexType *)he1->next->from_v, (VertexType *)he2->from_v));
		}
*/		

		std::deque<FaceType * > face_deque ; 
		std::map <VertexType *, VertexType * > vertex_map ;

		// Actually, we have to specifiy a correct starting traversing face. 
		//FaceType * begin_face = (FaceType *) this->faces[0]; 
		face_deque.push_back (begin_face);
		begin_face->accessed = true; 
		unsigned count = 0; 
		while (face_deque.size())
		{
			FaceType * face = face_deque[0] ;
			face_deque.pop_front();

			std::vector<HalfEdgeType *> v_he ;
			face->Face2Halfedge(v_he);

			// Create new face here
			count ++ ;
			copy_face (new_mesh, v_he, vertex_map, boundary_vertex_map, face->normalVector) ;

			for (unsigned i = 0; i < v_he.size(); ++i)
			{
				if (v_he[i]->flip != NULL && ((FaceType *) (v_he[i]->flip->face))->accessed != true)
				{
					((FaceType * )v_he[i]->flip->face)->accessed = true; 
					face_deque.push_back((FaceType * )v_he[i]->flip->face);
				}
			}
		}
		std::cout<<"Traverse count "<<count<<std::endl;
		
	}

	
	
private :

	int add_vertex (HalfEdgeType * he)
	{
		unsigned start_idx = this->vertices.size(); 
		
		if (false == he->is_allocated())
		{
			VertexType * pv = new VertexType ;
			this->vertices[start_idx] = pv; 
			pv->m_coord = he->m_intr_point ;
			he->set_point_idx(start_idx);
			return start_idx; 
		}
		else
		{
			return he->get_point_idx();
		}		
	}

	VertexType * create_new_vertex (const PointType & pt)
	{
		VertexType * pv = new VertexType ;
		pv->m_coord = pt;
		pv->mp_mesh = this; 
		pv->m_idx =  this->vertices.size();
		this->vertices[pv->m_idx] = pv; 
		VertexType * tag_vptr = pv;   
		return tag_vptr; 
	}

	VertexType * create_new_vertex (VertexType * & tag_vptr, const PointType & pt)
	{
		if (tag_vptr == NULL)
		{
			VertexType * pv = new VertexType ;
			pv->m_coord = pt;
			pv->mp_mesh = this; 
			pv->m_idx =  this->vertices.size();
			this->vertices[pv->m_idx] = pv; 
			tag_vptr = pv;   
		}
		return tag_vptr; 
	}

	VertexType * add_edge_vertex (EdgeType * edge, VertexType * & tag_vptr, const PointType & pt)
	{
		unsigned size = edge->v_edge_point.size() ;
		for (unsigned i = 0; i < size; ++i)
		{
			// Find one existing point 
			if ((edge->v_edge_point[i].second->mp_mesh == this) && (pt - edge->v_edge_point[i].first).square_module() < sq_tiny_value )
			{
				tag_vptr = edge->v_edge_point[i].second ;
				return tag_vptr; 
			}
		}
		VertexType * pv = create_new_vertex (tag_vptr, pt);
		edge->v_edge_point.push_back (std::make_pair (pt, pv) );
		return pv ;
	}

	VertexType * add_face_vertex (FaceType * face, VertexType * & tag_vptr, const PointType & pt)
	{
		unsigned size = face->v_face_point.size() ;
		for (unsigned i = 0; i < size; ++i)
		{
			// Find one existing point 
			if ((face->v_face_point[i].second->mp_mesh == this) && (pt - face->v_face_point[i].first).square_module() < sq_tiny_value )
			{
				tag_vptr = face->v_face_point[i].second ;
				return tag_vptr; 
			}
		}
		VertexType * pv = create_new_vertex (tag_vptr, pt);
		face->v_face_point.push_back (std::make_pair (pt, pv) );
		return pv ;
	}
private:


public:
	
	void sort_he_point (std::vector<PointType> & pt_v, const PointType & ref_point) 
	{
		unsigned size = pt_v.size(); 
		for (unsigned iPos = 0; iPos < size; iPos++)
		{
			unsigned iMin = iPos;
			for (unsigned i = iPos+1; i < size; i++)
			{
				if ((pt_v[i] - ref_point).square_module() < (pt_v[iMin] - ref_point).square_module())
				{
						iMin = i;
				}
			}

			if ( iMin != iPos )
			{
				std::swap(pt_v[iPos], pt_v[iMin]);
			}
		}
	}
	void filter_he_duplicated_point (std::vector<PointType> & pt_v)
	{
		// Filter the duplicated vertices. 
		Vector<Value> first_point = pt_v[0] ;
		unsigned pos = 0; 
		for (unsigned i = 1; i < pt_v.size() ; ++i)
		{
			if ((pt_v[i] - pt_v[pos]).square_module() < sq_tiny_value) continue; 
			pt_v[pos + 1] = pt_v[i] ;  
			pos++ ;
		}
		pt_v.erase (pt_v.begin() + pos + 1, pt_v.end());
	
	}

	void find_next_point (PointType & current_p, const HalfEdgeType * current_he, PointType & next_point, VertexType * & next_vpt, 
						  HalfEdgeType * & next_he, bool & isArrayEnd)
	{
		// Is the point a vertex
		if ((current_p - current_he->from_v->m_coord ).square_module() < sq_tiny_value )
		{
			if (current_he->m_points.size() > 0 ) 
			{
				next_he = (HalfEdgeType *)current_he; 
				next_point = current_he->m_points[0] ;
				isArrayEnd = true; 
				return ;
			}
		}

		int current_idx = -1; 
		for (unsigned i = 0 ; i < current_he->m_points.size(); ++i)
		{
			if ((current_p - current_he->m_points[i]).square_module()< sq_tiny_value)
			{
				current_idx  = int(i) ; 
				break; 
			}
		}
		if (current_idx < int (current_he->m_points.size() - 1))
		{
			next_he = (HalfEdgeType * )current_he; 
			next_point = current_he->m_points[current_idx + 1] ;
			isArrayEnd = true; 
			return ;						
		}
		else 
		{
			next_he = (HalfEdgeType * )current_he->next;  
			next_point = current_he->next->from_v->m_coord ;
			next_vpt = (VertexType *) current_he->next->from_v ;
			isArrayEnd = false; 
			return ;
		}
		// The point is the end vertex ;
		find_next_point (current_p, (HalfEdgeType *)current_he->next, next_point , next_vpt, next_he, isArrayEnd);
	}

	bool classify_face (std::vector<std::vector<Vector <ValueType> > > & v_curves, std::vector<std::pair<HalfEdgeType *, HalfEdgeType *> > & v_hes, std::vector<std::pair<bool, bool> > & v_curve_acc, std::vector<std::deque<VertexType * > > & v_curve_vptr , PointType & begin_point, VectorType & normal_vector)
	{

		HalfEdgeType * end_he;
		PointType current_point = begin_point ;
		PointType next_point = current_point ;
		HalfEdgeType * next_he  ;
		bool isArrayEnd = true; 
		VertexType * next_vpt; 

		int curve_idx, end_flag; 
		find_array_end (v_curves, v_curve_acc, current_point, curve_idx, end_flag) ;

		if (end_flag == 0)
		{
			current_point =  *(-- v_curves[curve_idx].end() );
			end_he = v_hes[curve_idx].second;
		}
		else
		{
			current_point =  *(v_curves[curve_idx].begin() );						
			end_he = v_hes[curve_idx].first;
		}
		find_next_point (current_point, end_he, next_point, next_vpt, next_he, isArrayEnd);
		PointType edge_center = (current_point + next_point ) * ValueType(0.5f); 

		return p_mesh2->is_in_volumn(edge_center, normal_vector); 
	}


	void find_array_end (const std::vector<std::vector<Vector <ValueType> > > & v_curves, const std::vector<std::pair<bool, bool> > & v_curve_acc, const PointType & current, int & idx, int & flag)
	{
		for (unsigned i = 0; i < v_curves.size(); ++i)
		{
			if ( (current -  (*v_curves[i].begin())).square_module() < sq_tiny_value /* && v_curve_acc[i].first == true*/)
			{
				idx = i; 
				flag = 0; return ;
			}
			if ( (*(--v_curves[i].end()) - current ).square_module() < sq_tiny_value /*&& v_curve_acc[i].second == true*/)
			{
				idx = i; 
				flag = 1; return ;
			}
		}
		throw ("Didn't find the value ");
	}

	void build_one_face (const std::vector<std::vector<Vector <ValueType> > > & v_curves, const std::vector<std::pair<HalfEdgeType *, HalfEdgeType *> > & v_hes, std::vector<std::pair<bool, bool> > & v_curve_acc, std::vector<std::deque<VertexType * > > & v_curve_vptr , PointType & begin_point, const  VectorType & normal_vector, std::vector<std::deque<SegmentStruct * > > & v_segRecord)
	{		
		// traverse
		HalfEdgeType * end_he;
		PointType current_point = begin_point ;
		PointType next_point = current_point ;
		HalfEdgeType * next_he  ;
		bool isArrayEnd = true; 
		VertexType * next_vpt; 
		std::vector<unsigned > v_vertex_id; 
		FaceType * old_face = (FaceType *) v_hes[0].first->face; 

		// Record which segment struct should be built. 
		std::vector<SegmentStruct *> seg_record; 
		do 
		{
			if (isArrayEnd == true )
			{
				int curve_idx, end_flag; 
				find_array_end (v_curves, v_curve_acc, current_point, curve_idx, end_flag) ;
				std::cout<<"Current_point "<<current_point<<std::endl;
				std::cout<<"Curve idx "<<curve_idx<<std::endl;
				std::cout<<"end_flag "<<end_flag<<std::endl;
				if (end_flag == 0)
					v_curve_acc[curve_idx].first = true; 
				else 
					v_curve_acc[curve_idx].second = true; 
				unsigned curve_size = v_curves[curve_idx].size();
				// Start from left to right or from right to left. 
				if (end_flag == 0)
				{
					VertexType * pv ;
					// Add the first ;
					pv = add_edge_vertex((EdgeType *)(v_hes[curve_idx].first->edge), v_curve_vptr[curve_idx][0], v_curves[curve_idx][0]); v_vertex_id.push_back(pv->m_idx);
					seg_record.push_back (v_segRecord[curve_idx][0]);  // Record halfedge

					for (unsigned j = 1; j < curve_size - 1 ; ++j)
					{
						pv = create_new_vertex (v_curves[curve_idx][j]);
						pv = add_face_vertex(old_face, v_curve_vptr[curve_idx][j], v_curves[curve_idx][j]);
						v_vertex_id.push_back(pv->m_idx);
						seg_record.push_back (v_segRecord[curve_idx][j]); // Record halfedge
					}
					// Add the last
					pv = add_edge_vertex((EdgeType *)(v_hes[curve_idx].second->edge), v_curve_vptr[curve_idx][curve_size -1], v_curves[curve_idx][curve_size - 1]); v_vertex_id.push_back(pv->m_idx);
					current_point =  *(-- v_curves[curve_idx].end() );
					end_he = v_hes[curve_idx].second;
					seg_record.push_back((SegmentStruct * )(NULL));  // Place holder
				}
				else
				{
					// Add the first
					VertexType * pv ;
					pv = add_edge_vertex((EdgeType *)(v_hes[curve_idx].second->edge), v_curve_vptr[curve_idx][curve_size -1], v_curves[curve_idx][curve_size - 1]); v_vertex_id.push_back(pv->m_idx);
					seg_record.push_back (v_segRecord[curve_idx][curve_size - 2]);  // Record halfedge
					for (int j = int( curve_size - 2) ; j >= 1; --j)
					{
						pv = create_new_vertex(v_curves[curve_idx][j]);
						pv = add_face_vertex(old_face, v_curve_vptr[curve_idx][j], v_curves[curve_idx][j]);
						v_vertex_id.push_back(pv->m_idx);
						seg_record.push_back (v_segRecord[curve_idx][j - 1]);  // Record halfedge
					}
					pv = add_edge_vertex((EdgeType *)(v_hes[curve_idx].first->edge), v_curve_vptr[curve_idx][0], v_curves[curve_idx][0]);v_vertex_id.push_back(pv->m_idx);

					current_point =  *(v_curves[curve_idx].begin() );					  
					end_he = v_hes[curve_idx].first;
					seg_record.push_back((SegmentStruct * )(NULL)); // Place holder
				}
				std::cout<<"curve_end_point "<<current_point<<std::endl;
				std::cout<<"End          he "<<end_he<<std::endl;
			}

			find_next_point (current_point, end_he, next_point, next_vpt, next_he, isArrayEnd);
			
			std::cout<<"Next Point"<<next_point<<std::endl;
			std::cout<<"Next He   "<<next_he<<std::endl;
			current_point = next_point ;
			end_he = next_he ;
			if (next_point != begin_point && isArrayEnd == false)
			{
				v_vertex_id.push_back(next_vpt->m_idx);
				seg_record.push_back((SegmentStruct * )(NULL));
			}
			
			//traverse along the end_he 
		} while (next_point != begin_point ) ;
		
		
		// Create New Face Here;
		FaceType * pface = this->construct_face( v_vertex_id); 
		pface->normalVector = normal_vector; 

		// Record half edge record for merging
		for (unsigned i = 0; i < v_vertex_id.size(); ++i) 
		{
			if (seg_record[i])
				add_intersection_record (seg_record[i], (HalfEdgeType *)this->vertices[v_vertex_id[i]]->he) ;
		}

		// Add the face id into new face
		this->v_new_face_id.push_back(pface->m_idx );


		//std::cout<<"Size Size "<<v_vertex_id.size()<<' '<<seg_record.size()<<std::endl;
		std::cout<<"Face "<<pface->m_idx<<' ' ;
		for (unsigned i = 0; i < v_vertex_id.size(); ++i)
		{
			std::cout<<v_vertex_id[i]<<':'<<this->vertices[v_vertex_id[i] ]->m_coord<<' ';
		}
		std::cout<<std::endl;
	}

	void build_one_face_coplanar (const std::vector<std::vector<Vector <ValueType> > > & v_curves, const std::vector<std::pair<HalfEdgeType *, HalfEdgeType *> > & v_hes, std::vector<std::pair<bool, bool> > & v_curve_acc, std::vector<std::deque<VertexType * > > & v_curve_vptr , PointType & begin_point, const  VectorType & normal_vector, std::vector<std::deque<SegmentStruct * > > & v_segRecord)
	{		
		// traverse
		HalfEdgeType * end_he;
		PointType current_point = begin_point ;
		PointType next_point = current_point ;
		HalfEdgeType * next_he  ;
		bool isArrayEnd = true; 
		VertexType * next_vpt; 
		std::vector<unsigned > v_vertex_id; 
		FaceType * old_face = (FaceType *) v_hes[0].first->face; 

		// Record which segment struct should be built. 
		std::vector<SegmentStruct *> seg_record; 
		do 
		{
			if (isArrayEnd == true )
			{
				int curve_idx, end_flag; 
				find_array_end (v_curves, v_curve_acc, current_point, curve_idx, end_flag) ;
				std::cout<<"Current_point "<<current_point<<std::endl;
				std::cout<<"Curve idx "<<curve_idx<<std::endl;
				std::cout<<"end_flag "<<end_flag<<std::endl;
				if (end_flag == 0)
					v_curve_acc[curve_idx].first = true; 
				else 
					v_curve_acc[curve_idx].second = true; 
				unsigned curve_size = v_curves[curve_idx].size();
				// Start from left to right or from right to left. 
				if (end_flag == 0)
				{
					VertexType * pv ;
					// Add the first ;
					pv = add_edge_vertex((EdgeType *)(v_hes[curve_idx].first->edge), v_curve_vptr[curve_idx][0], v_curves[curve_idx][0]); v_vertex_id.push_back(pv->m_idx);
					//seg_record.push_back (v_segRecord[curve_idx][0]);  // Record halfedge

					for (unsigned j = 1; j < curve_size - 1 ; ++j)
					{
						pv = create_new_vertex (v_curves[curve_idx][j]);
						pv = add_face_vertex(old_face, v_curve_vptr[curve_idx][j], v_curves[curve_idx][j]);
						v_vertex_id.push_back(pv->m_idx);
						//seg_record.push_back (v_segRecord[curve_idx][j]); // Record halfedge
					}
					// Add the last
					pv = add_edge_vertex((EdgeType *)(v_hes[curve_idx].second->edge), v_curve_vptr[curve_idx][curve_size -1], v_curves[curve_idx][curve_size - 1]); v_vertex_id.push_back(pv->m_idx);
					current_point =  *(-- v_curves[curve_idx].end() );
					end_he = v_hes[curve_idx].second;
					//seg_record.push_back((SegmentStruct * )(NULL));  // Place holder
				}
				else
				{
					// Add the first
					VertexType * pv ;
					pv = add_edge_vertex((EdgeType *)(v_hes[curve_idx].second->edge), v_curve_vptr[curve_idx][curve_size -1], v_curves[curve_idx][curve_size - 1]); v_vertex_id.push_back(pv->m_idx);
					//seg_record.push_back (v_segRecord[curve_idx][curve_size - 2]);  // Record halfedge
					for (int j = int( curve_size - 2) ; j >= 1; --j)
					{
						pv = create_new_vertex(v_curves[curve_idx][j]);
						pv = add_face_vertex(old_face, v_curve_vptr[curve_idx][j], v_curves[curve_idx][j]);
						v_vertex_id.push_back(pv->m_idx);
						//seg_record.push_back (v_segRecord[curve_idx][j - 1]);  // Record halfedge
					}
					pv = add_edge_vertex((EdgeType *)(v_hes[curve_idx].first->edge), v_curve_vptr[curve_idx][0], v_curves[curve_idx][0]);v_vertex_id.push_back(pv->m_idx);

					current_point =  *(v_curves[curve_idx].begin() );					  
					end_he = v_hes[curve_idx].first;
					//seg_record.push_back((SegmentStruct * )(NULL)); // Place holder
				}
				std::cout<<"curve_end_point "<<current_point<<std::endl;
				std::cout<<"End          he "<<end_he<<std::endl;
			}

			find_next_point (current_point, end_he, next_point, next_vpt, next_he, isArrayEnd);
			
			std::cout<<"Next Point"<<next_point<<std::endl;
			std::cout<<"Next He   "<<next_he<<std::endl;
			current_point = next_point ;
			end_he = next_he ;
			if (next_point != begin_point && isArrayEnd == false)
			{
				v_vertex_id.push_back(next_vpt->m_idx);
				//seg_record.push_back((SegmentStruct * )(NULL)); // Place holder
			}
			
			//traverse along the end_he 
		} while (next_point != begin_point ) ;
		
		
		// Create New Face Here;
		FaceType * pface = this->construct_face( v_vertex_id); 
		pface->normalVector = normal_vector; 

		// Record half edge record for merging
		for (unsigned i = 0; i < v_vertex_id.size(); ++i) 
		{
			/*
			if (seg_record[i])
				add_intersection_record (seg_record[i], (HalfEdgeType *)this->vertices[v_vertex_id[i]]->he) ;
			*/
		}

		// Add the face id into new face
		this->v_new_face_id.push_back(pface->m_idx );


		//std::cout<<"Size Size "<<v_vertex_id.size()<<' '<<seg_record.size()<<std::endl;
		std::cout<<"Face "<<pface->m_idx<<' ' ;
		for (unsigned i = 0; i < v_vertex_id.size(); ++i)
		{
			std::cout<<v_vertex_id[i]<<':'<<this->vertices[v_vertex_id[i] ]->m_coord<<' ';
		}
		std::cout<<std::endl;
	}

	void add_intersection_record (SegmentStruct * p_segstr, HalfEdgeType * he)
	{
		typename std::map <std::pair <unsigned, unsigned>, IntersectionRecord<Value> >::iterator iter ;
		iter = map_mergeInter.find (p_segstr->face_key);

		if (iter == map_mergeInter.end())
		{
			IntersectionRecord<Value> ir ;
			ir.type = SEGMENT ;
			ir.v_merge_he.resize (1);
			ir.v_merge_he[0].first = he;
			ir.segment = Segment<Value>(he->from_v->m_coord , he->next->from_v->m_coord ) ;
			map_mergeInter.insert (std::make_pair ( p_segstr->face_key, ir));
		}
		else
		{
			throw ("Invalid key");
		}
	}

	void build_faces (const FaceType * face , std::vector<std::vector<Vector<Value> > >& cut_line_set, std::vector<std::pair<HalfEdgeType *, HalfEdgeType *> > & cut_he_set, std::vector<bool> & cut_line_type, std::vector<std::deque<SegmentStruct * > > & v_segRecord)
	{
		std::cout<<"Cut face here "<<std::endl;		
		std::vector<std::vector<Vector<ValueType> > > & v_curves  = cut_line_set  ; 
		std::vector<std::pair<HalfEdgeType *, HalfEdgeType *> > & v_hes = cut_he_set ;
		
		unsigned size = v_curves.size(); 
		std::set<HalfEdgeType *> he_set ;
		// Add all the point to the halfedge 
		for (unsigned i = 0; i < size; ++i)
		{
			v_hes[i].first->m_points.push_back(v_curves[i][0]);
			if (v_hes[i].first->mp_mesh != this) throw ("ERROR 01");
			v_hes[i].second->m_points.push_back(v_curves[i][v_curves[i].size() - 1]);
			if (v_hes[i].second->mp_mesh != this) throw ("ERROR 02");
			he_set.insert (v_hes[i].first);
			he_set.insert (v_hes[i].second);
		}
		
		// Sort the point on the halfedges . 
		std::cout<<"Sort the point on the halfedges"<<std::endl;
		for (typename std::set<HalfEdgeType *>::iterator iter =  he_set.begin(); 
			 iter != he_set.end(); ++iter)
		{
			std::cout<<"HE: "<<(*iter)<<std::endl;
			PointType ref_point = (*iter)->from_v->m_coord; 
			//sort ((*iter)->m_points.begin(), (*iter)->m_points.end(), order);
			sort_he_point ((*iter)->m_points, ref_point);  // Sort the point on halfedges.
			//filter_he_duplicated_point((*iter)->m_points); // Filter duplicated point. 
			unsigned p_size = (*iter)->m_points.size();
			for (unsigned i = 0; i < p_size ; ++i)
			{
				std::cout<<(*iter)->m_points[i]<<' ' <<((*iter)->m_points[i]- ref_point).square_module();
			}
			std::cout<<endl;
		}
		
		// For each curve trace its two side
		// The first tag indicates from left to right. 
		// The second tag indicates from right to left. 
		std::vector<std::pair<bool, bool> > v_curve_acc ;
		std::vector<std::deque<VertexType * > > v_curve_vptr (size); 
		for (unsigned i = 0; i < size; ++i) 
		{
			v_curve_acc.push_back(std::make_pair(false, false));
			//	v_curve_vptr.push_back (std::make_pair (, (VertexType *)NULL));
			for (unsigned j = 0; j < v_curves[i].size(); ++j)
			{
				v_curve_vptr[i].push_back((VertexType *)NULL) ;
			}
		}
		
		for (unsigned i = 0; i < size; ++i )
		{
			HalfEdgeType * begin_he = v_hes[i].first;
			VectorType normal_vector = begin_he->face->normalVector; 
			// Trace in one direction; 
			if (v_curve_acc[i].first == false)
			{
				std::cout<<"Traverse in one direction. "<<std::endl;
				v_curve_acc[i].first = true; 
				PointType begin_point = v_curves[i][0];

				if (cut_line_type[i] == false )
				{
					if (!classify_face (v_curves, v_hes, v_curve_acc, v_curve_vptr, begin_point, normal_vector))			
					{
						build_one_face (v_curves, v_hes, v_curve_acc, v_curve_vptr, begin_point, normal_vector, v_segRecord);						
					}		
				}
				else 
				{
					if (filter_coplanar == false)
					{
						build_one_face_coplanar (v_curves, v_hes, v_curve_acc, v_curve_vptr, begin_point, normal_vector, v_segRecord);						
					} 
					else if (!classify_face (v_curves, v_hes, v_curve_acc, v_curve_vptr, begin_point, normal_vector))			
					{
						build_one_face_coplanar (v_curves, v_hes, v_curve_acc, v_curve_vptr, begin_point, normal_vector, v_segRecord);						
					}	
				}


				
			}
			// Trace in the other direction; 
			if (v_curve_acc[i].second == false)
			{
				std::cout<<"Traverse in the other direction. "<<std::endl;
				v_curve_acc[i].second = true; 
				PointType begin_point = v_curves[i][v_curves[i].size() -1]; 

				if (cut_line_type[i] == false )
				{
					if (!classify_face (v_curves, v_hes, v_curve_acc, v_curve_vptr, begin_point, normal_vector))			
					{
						build_one_face (v_curves, v_hes, v_curve_acc, v_curve_vptr, begin_point, normal_vector, v_segRecord);						
					}		
				}
				else 
				{
					if (filter_coplanar == false)
					{
						build_one_face_coplanar (v_curves, v_hes, v_curve_acc, v_curve_vptr, begin_point, normal_vector, v_segRecord);						
					} 
					else if (!classify_face (v_curves, v_hes, v_curve_acc, v_curve_vptr, begin_point, normal_vector))			
					{
						build_one_face_coplanar (v_curves, v_hes, v_curve_acc, v_curve_vptr, begin_point, normal_vector, v_segRecord);						
					}	
				}

			}
		}
		// clear 
		for (typename std::set<HalfEdgeType *>::iterator iter =  he_set.begin(); 
			 iter != he_set.end(); ++iter)
		{
			(*iter)->m_points.clear();
		}
	}

	void find_line (std::vector<SegmentStruct> & seg_v, unsigned pos, unsigned lr, std::vector<std::pair<HalfEdgeType *, HalfEdgeType *> >  & v_he,  std::deque<PointType> & deq_curve, std::deque<HalfEdgeType * > & deq_he, 		std::deque<SegmentStruct * > & deq_segRecord)
	{
		seg_v[pos].acc[lr] = true; 
		PointType  point ; // , pick_point;
		if (lr == 0) 
		{
			point = seg_v[pos].segment.p0;  
			deq_he.push_back((HalfEdgeType * )v_he[pos].first); 
		}
		else 
		{
			point = seg_v[pos].segment.p1;
			deq_he.push_back((HalfEdgeType * )v_he[pos].second);
		}
		deq_curve.push_back(point);
		std::cout<<"POINT "<<point<<std::endl;

		// Find in one direction.
	    std::cout<<"Find in one direction"<<std::endl;
		while(1) 
		{
			bool flag = true; 
			for (unsigned i = 0; i < seg_v.size(); ++i)
			{
				if (seg_v[i].acc[0] == false && (seg_v[i].segment.p0 - point).square_module() < sq_tiny_value)
				{
					seg_v[i].acc[0] = true; 
					seg_v[i].acc[1] = true; 
					point = seg_v[i].segment.p0;		std::cout<<point;
					point = seg_v[i].segment.p1;		std::cout<<point<<std::endl;
					deq_curve.push_back(point);
					deq_he.push_back((HalfEdgeType * )v_he[i].second); 
					deq_segRecord.push_back (& seg_v[i]);
					flag = false; 
					break; 
				}
				else if (seg_v[i].acc[1] == false && (seg_v[i].segment.p1 - point).square_module() < sq_tiny_value)
				{
					seg_v[i].acc[1] = true; 
					seg_v[i].acc[0] = true; 
					point = seg_v[i].segment.p1;		std::cout<<point;
					point = seg_v[i].segment.p0;		std::cout<<point<<std::endl;
					deq_curve.push_back(point);
					deq_he.push_back((HalfEdgeType * )v_he[i].first); 
					deq_segRecord.push_back (& seg_v[i]);
					flag = false; 
					break; 
				}
			}
			if (flag == true) break; 
		}
		// Find in another direction. 
		std::cout<<"Find in the other direction"<<std::endl;
		if (lr == 0 && seg_v[pos].acc[1] == false) 
		{
			point = seg_v[pos].segment.p1; 
			seg_v[pos].acc[1] = true;
			deq_he.push_front((HalfEdgeType * )v_he[pos].second); 
			deq_curve.push_front(point);
			deq_segRecord.push_front (& seg_v[pos]);
			std::cout<<"POINT "<<point<<std::endl;

		}
		else  if (lr == 1 && seg_v[pos].acc[0] == false) 
		{
			point = seg_v[pos].segment.p0;
			seg_v[pos].acc[0] = true; 
			deq_he.push_front((HalfEdgeType * )v_he[pos].first); 
			deq_curve.push_front(point);
			deq_segRecord.push_front (& seg_v[pos]);
			std::cout<<"POINT "<<point<<std::endl;
		}

		//point = pick_point ;
		while(1) 
		{
			bool flag = true; 
			for (unsigned i = 0; i < seg_v.size(); ++i)
			{
				if (seg_v[i].acc[0] == false && (seg_v[i].segment.p0 - point).square_module() < sq_tiny_value)
				{
					seg_v[i].acc[0] = true; 
					seg_v[i].acc[1] = true; 
					point = seg_v[i].segment.p0;		std::cout<<point;
					point = seg_v[i].segment.p1;		std::cout<<point<<std::endl;
					deq_curve.push_front(point);
					deq_he.push_front((HalfEdgeType * )v_he[i].second); 
					deq_segRecord.push_front (& seg_v[i]);
					flag = false; 
					break; 
				}
				else if (seg_v[i].acc[1] == false && (seg_v[i].segment.p1 - point).square_module() < sq_tiny_value)
				{
					seg_v[i].acc[1] = true; 
					seg_v[i].acc[0] = true; 
					point = seg_v[i].segment.p1;		std::cout<<point;
					point = seg_v[i].segment.p0;		std::cout<<point<<std::endl;
					deq_curve.push_front(point);
					deq_he.push_front((HalfEdgeType * )v_he[i].first); 
					deq_segRecord.push_front (& seg_v[i]);
					flag = false; 
					break; 
				}
			}
			if (flag == true) break; 
		}
	}
    void populate_segment_intersect (const PolygonType & _pol, std::vector<SegmentStruct > & v_seg, std::vector<std::pair<HalfEdgeType *, HalfEdgeType *> >  & v_he, std::vector<std::deque<Vector <ValueType> > > & v_curves, 		std::vector<std::deque<HalfEdgeType * > >  & v_hes, std::vector<std::deque<SegmentStruct * > > & v_segRecord)
	{
		std::cout<<"Polygon "<<_pol<<std::endl;
		std::cout<<"SEG V Begin"<<std::endl;
		for (unsigned i = 0; i < v_seg.size() ; ++i)
		{
			std::cout<<v_seg[i].segment<<std::endl;
			if (v_he[i].first->mp_mesh == this) std::cout<<"I " ; else std::cout<<"O ";
			if (v_he[i].second->mp_mesh == this) std::cout<<"I " ; else std::cout<<"O ";
			std::cout<<std::endl;
		}
		std::cout<<"SEG V End  "<<std::endl;		
		/*
		std::deque<PointType> deq_curve ;
		std::deque<HalfEdgeType * > deq_he ;
		find_line (seg_v, 17, 0, v_he,  deq_curve, deq_he) ;
		unsigned count = 0;
		std::cout<<"**********"<<std::endl;
		for (typename std::deque<PointType>::iterator iter = deq_curve.begin(); iter != deq_curve.end(); ++iter )
		{
			std::cout<<count++<<(*iter)<<std::endl;
		}
		*/
		
		while (1)
		{

			bool isAcc = true; 
			std::deque<PointType> deq_curve ;
			std::deque<HalfEdgeType * > deq_he ;
			std::deque<SegmentStruct * > deq_segRecord; 
			for (unsigned i = 0; i < v_seg.size(); ++i)
			{
				if (v_seg[i].acc[0] == false)
				{
					//find_line (seg_v, i, 0, v_he,  deq_curve, deq_he) ;
					find_line (v_seg, i, 0, v_he,  deq_curve, deq_he, deq_segRecord) ;
					isAcc = false; 
					break; 
				}
				if (v_seg[i].acc[1] == false)
				{
					//find_line (seg_v, i, 1, v_he,  deq_curve, deq_he) ;
					find_line (v_seg, i, 1, v_he,  deq_curve, deq_he, deq_segRecord) ;
					isAcc = false; 
					break; 
				}
			}
			if (isAcc == true ) break; 
			unsigned count = 0;
			std::cout<<"********** CURVE ********"<<std::endl;
			for (typename std::deque<PointType>::iterator iter = deq_curve.begin(); iter != deq_curve.end(); ++iter )
			{
				std::cout<<count++<<(*iter)<<std::endl;
			}
			for (unsigned i = 0; i < deq_segRecord.size(); ++i) std::cout<< "SEG "<<i<<' '<<deq_segRecord[i]->segment<<std::endl;

			v_curves.push_back(deq_curve);
			v_hes.push_back(deq_he);
			v_segRecord.push_back(deq_segRecord);
		}
	}

public :

	void mesh_intersection_relation (MeshType & mesh2)
	{
		this->tree2 = mesh2.get_aabbtree() ;
		MeshType & mesh1 = *this ;
		typename AABBTreePolyType::PolygonColResult polygonColResult; 

		AABBTreePolyType * tree2 = mesh2.get_aabbtree();


		//for (MeshType::FaceIterator iter = mesh1.faces.begin(); iter != mesh1.faces.end(); ++iter)
		unsigned size1 = mesh1.faces.size();
		for (unsigned mesh1_id = 0; mesh1_id < size1; ++mesh1_id)
		{		

			const PolygonType & p1 = mesh1.faces[mesh1_id]->get_polygon (); 
			tree2->collideWithPolygon (p1, polygonColResult);
			if (polygonColResult.polys_.size() > 0)
			{
				v_face_id.insert(mesh1_id);
				for (unsigned i = 0; i < polygonColResult.polys_.size(); ++i)
				{
					unsigned mesh2_id = polygonColResult.polys_[i]->get_face_idx(); 
					this->map_mesh1to2.insert (std::make_pair (mesh1_id, mesh2_id));
					mesh2.map_mesh1to2.insert (std::make_pair (mesh2_id, mesh1_id));
					//this->key_mesh1.insert (mesh1_id);
					//this->key_mesh2.insert (mesh2_id);
				}
			}
			polygonColResult.polys_.clear();
		}
/*
		std::cout<<"Relation Begin "<<std::endl;
		for (MapUUIterator iter = map_mesh1to2.begin(); iter != map_mesh1to2.end(); ++iter)
		{
			std::cout<<iter->first<<' '<< iter->second<<std::endl;
		}
		std::cout<<"Relation End "<<std::endl;
*/
	}
	/**
	 * If we can merge line1 to line2, return true. otherwise return false. 
	 */
	bool merge_pair_lines (std::vector<Vector<Value> > & line1 , std::vector<Vector<Value> > & line2)
	{
		for_each (line1.begin(), line1.end(), std::cout<<boost::lambda::_1<<' '); std::cout<<'|';
		for_each (line2.begin(), line2.end(), std::cout<<boost::lambda::_1<<' '); std::cout<<std::endl;
		// Judge in one direction. 
		for (unsigned i = 0 ; i < line2.size(); ++i)
		{
			if ((line1[0] - line2[i]).square_module() < sq_tiny_value && i + line1.size() <= line2.size())
			{
				bool inflag = true; 
				for (unsigned j = 0; j < line1.size(); ++j)
				{
					if ((line1[j] - line2[j + i ]).square_module() < sq_tiny_value) ; 
					else{ inflag = false ; break; }
				}
				if (inflag) return true; 
			}			
		}
		// judge in the other direction. 
		for (int i = int(line2.size()) - 1 ; i >= 0; --i)
		{
			if ((line1[0] - line2[i]).square_module() < sq_tiny_value && i + 1 >= int(line1.size())  )
			{
				bool inflag = true; 
				for (unsigned j = 0; j < line1.size(); ++j)
				{
					if ((line1[j] - line2[i - j ]).square_module() < sq_tiny_value) ; 
					else{ inflag = false ; break; }
				}
				if (inflag) return true; 
			}			
		}
		return false ;
	}
	void merge_cut_lines (std::vector<std::vector<Vector<Value> > >& cut_line_set, std::vector<std::pair<HalfEdgeType *, HalfEdgeType *> > & cut_he_set)
	{
		std::cout<<"Merge Cutlines"<<std::endl;
		std::vector<std::vector<Vector<Value> > > new_cut_set; 
		std::vector<std::pair<HalfEdgeType *, HalfEdgeType *> > new_he_set; 
		for (unsigned i = 0; i< cut_line_set.size(); ++i)
		{
			bool flag = false; 
			for (unsigned j = 0; j < new_cut_set.size(); ++j)
			{
				if (merge_pair_lines (cut_line_set[i], new_cut_set[j]) ) 
				{
					flag = true; 
					break; 
				}				
			}
			if (flag == false) {new_cut_set.push_back(cut_line_set[i]) ; new_he_set.push_back (cut_he_set[i]) ;}
		}
		cut_line_set = new_cut_set; 
		cut_he_set = new_he_set ;
	}
	void face_splitter(MapUUIterator begin_iter, MapUUIterator end_iter, MeshType & mesh1, MeshType & mesh2)
	{
		v_segment.clear(); 
		v_halfedge.clear();
		bool coplanar = false; 
		std::vector<std::vector<Vector<Value> > > cut_line_set ;
		std::vector<bool> cut_line_type; // iscoplanar or not ;
		std::vector<std::pair<HalfEdgeType *, HalfEdgeType *> > cut_he_set ;
		FaceType * split_face = NULL;
		unsigned id1, id2;
		for (MapUUIterator in_map_iter = begin_iter; in_map_iter != end_iter; ++ in_map_iter )
		{
			id1 = in_map_iter->first ;
			id2 = in_map_iter->second ;
			std::cout<<"\n\nFace Splitter ["<<id1<<' '<<id2<<"] ";
			split_face = mesh1.faces[in_map_iter->first];
			//const PolygonType & p1 = this->faces[in_map_iter->first]->get_polygon (); 
			//const PolygonType & p2 = p_mesh2->faces[in_map_iter->second]->get_polygon (); 
			BooleanMeshIntersectResult<ValueType> intrRes; 

			// This flag should always be true. 
			bool flag = intersect_face_face (mesh1.faces[in_map_iter->first], mesh2.faces[in_map_iter->second], intrRes, coplanar) ;

			if (flag)
			{
				if (!coplanar)
				{
					if (intrRes.res_type == SEGMENT)
					{
						SegmentStruct segstr; 
						segstr.segment = intrRes.segment ;
						segstr.face_key = std::make_pair (id1, id2);
						segstr.count = 0; 
						v_segment.push_back(segstr) ;
						v_halfedge.push_back(make_pair (intrRes.source_he, intrRes.target_he));
						std::cout<<"Segment "<<intrRes.segment<<std::endl; 
					}
					else if (intrRes.res_type == POINT)
					{
						std::cout<<"Point type intersection";						
					}
					else
					{
						throw ("Cannot handle polygon. 1");
					}
				}
				else
				{
					if (intrRes.res_type == POLYGON)
					{
						//throw ("POLYGON Intersection");
						// Populate face face intersect
						cut_line_set.insert (cut_line_set.end(), intrRes.cut_set.begin(), intrRes.cut_set.end());
						cut_he_set.insert (cut_he_set.end(), intrRes.he_set.begin(), intrRes.he_set.end());
					}
					else
					{
						throw ("Cannot handle polygon. 2");
					}
				}
			}
			else
			{
				std::cout<<"id pair "<<id1<<' '<<id2<<std::endl; throw ("Inconsitency on predicator.");
			}
		}

		// sort the face-face intersection
		std::cout<<"\nCut lines begin"<<std::endl;		
		for (unsigned i = 0; i < cut_line_set.size(); ++i)
		{
			
			for (unsigned j = 0; j < cut_line_set[i].size(); ++j)
			{
				std::cout<<cut_line_set[i][j] <<' ';
			}
			std::cout<<" HE ";
			std::cout<< cut_he_set[i].first <<' '
					 << cut_he_set[i].second ;
						
			std::cout<<std::endl;		   
		}
		std::cout<<"Cut lines end "<<std::endl;			
		merge_cut_lines (cut_line_set, cut_he_set);
		std::cout<<"\nNew Cut lines begin"<<std::endl;		
		for (unsigned i = 0; i < cut_line_set.size(); ++i)
		{
			
			for (unsigned j = 0; j < cut_line_set[i].size(); ++j)
			{
				std::cout<<cut_line_set[i][j] <<' ';
			}
			std::cout<<" HE ";
			std::cout<< cut_he_set[i].first <<' '
					 << cut_he_set[i].second ;
						
			std::cout<<std::endl;		   
		}
		std::cout<<"New Cut lines end "<<std::endl;
		cut_line_type.resize(cut_line_set.size()); // shrink the cut_line_type; 
		for (unsigned i = 0; i < cut_line_type.size(); ++i) cut_line_type[i] = true; 

		// sort the segment intersection
		std::vector<std::deque<Vector <ValueType> > > v_curves; 
		std::vector<std::deque<HalfEdgeType * > > v_hes; 
		std::vector<std::deque<SegmentStruct * > > v_segRecord; 
		populate_segment_intersect (split_face->get_polygon(), v_segment, v_halfedge, v_curves, v_hes, v_segRecord);

		// Adapter from v_curves to cutline ;
		for (unsigned i = 0; i< v_curves.size(); ++i)
		{
			std::vector<Vector <ValueType> > v (v_curves[i].begin(), v_curves[i].end());
			cut_line_type.push_back(false);
			cut_line_set.push_back (v);
			cut_he_set.push_back (std::make_pair(v_hes[i][0], v_hes[i][ v_hes[i].size() -1]));
		}

		build_faces(split_face , cut_line_set, cut_he_set, cut_line_type,  v_segRecord);		
	}

	/**
	 * Split the mesh along the curve. 
	 * @param filter_coplanar. True, filter the coplanar face; False, otherwise. 
	 */
	
	void split_mesh (MeshType & mesh1, MeshType & mesh2, bool filter_coplanar)
	{
		this->filter_coplanar = filter_coplanar ;
		typename std::multimap<unsigned, unsigned>::iterator map_iter, in_map_iter, key_begin_iter;
		if (this->map_mesh1to2.size() == 0) return ;

		map_iter = this->map_mesh1to2.begin() ; 
		key_begin_iter = map_iter ;
		unsigned key = map_iter->first; 
		++map_iter ;

		// Traversal the map in group with the same key
		for (; map_iter != this->map_mesh1to2.end(); ++ map_iter)
		{
			if (map_iter->first != key)
			{
				face_splitter(key_begin_iter, map_iter, mesh1, mesh2);
				// Update the iterator
				key_begin_iter =  map_iter;
				key = map_iter->first; 
			}
		}
		// Deal with the last group
		face_splitter(key_begin_iter, map_iter,  mesh1,  mesh2);
	}

private :
	std::vector<std::pair<HalfEdgeType * , HalfEdgeType * > >  v_halfedge ;

	std::multimap <unsigned, unsigned> map_mesh1to2; 
	//std::multimap <unsigned, unsigned> map_mesh2to1;

 
	AABBTreePolyType * tree ;
	AABBTreePolyType * tree2 ;



public:

	struct SegmentStruct
	{
		Segment <Value> segment ;
		std::pair <unsigned, unsigned> face_key; 
		unsigned count ;
		/**
		 * For sort segment 
		 */
		SegmentStruct() {acc[0]= acc[1] = false; }
		bool acc[2];
	};
	MeshType * p_mesh2 ;
	PARTTYPE pt_type ;

	std::set<int> v_face_id;
	std::vector<int> v_new_face_id ;

	//std::vector<SegmentType > v_segment ;
	std::vector<SegmentStruct> v_segment ;
	std::vector<Polygon <Value> > v_polygon ;

	std::set <EdgeType *> v_edges ;

	// This structure is used for merging intersection. 
	std::map <std::pair <unsigned, unsigned>, IntersectionRecord<Value> > map_mergeInter;

	bool filter_coplanar ;
};

	

#endif
